#ifndef GAMEENGINE_GEOMETRY_PLANE3D_HPP
#define GAMEENGINE_GEOMETRY_PLANE3D_HPP

#ifdef GAMEENGINE_NAMESPACES
namespace GameEngine {
#endif

//=================================================================================
// Implementation of the Plane3d class

//! Default constructor
/*!
 @note Class T should have a default constructor
 */
template <class T>
Plane3d<T>::Plane3d() {}

//! Constructor from an origin and a normal
/*!
 @param origin The origin of the plane
 @param normal The normal of the plane
 */
template <class T>
Plane3d<T>::Plane3d(const Point3d<T>& origin, const Vector3d<T>& normal) : origin_(origin), normal_(normal) {}

//! Constructor from three points
/*!
 @param p1 The first point
 @param p2 The second point
 @param p3 The third point
 */
template <class T>
Plane3d<T>::Plane3d(const Point3d<T>& p1, const Point3d<T>& p2, const Point3d<T>& p3) :
	origin_(p1), normal_( Vector3d<T>(p1,p2).cross(Vector3d<T>(p1,p3)) ) {}

//! Copy constructor
/*!
 @param rhs The plane to copy
 @note Class T should have a copy constructor
 */
template <class T>
Plane3d<T>::Plane3d(const Plane3d& rhs) : origin_(rhs.origin_), normal_(rhs.normal_) {}

//! Destructor
/*!
 @note The destructor of class T should be public
 */
template <class T>
Plane3d<T>::~Plane3d() {}

//! Assignment operator
/*!
 @param rhs The plane to copy
 @note Class T should have an assignment operator
 */
template <class T>
Plane3d<T>& Plane3d<T>::operator=(const Plane3d& rhs) {
	if ( &rhs != this ) {
		origin_ = rhs.origin_;
		normal_ = rhs.normal_;
	}
	return *this;
}

//! Accessor to the origin of the plane (read-write)
/*!
 @return A reference to the origin of the plane
 */
template <class T>
Point3d<T>& Plane3d<T>::origin() {
	return origin_;
}

//! Accessor to the origin of the plane (read only)
/*!
 @return A constant reference to the origin of the plane
 */
template <class T>
const Point3d<T>& Plane3d<T>::origin() const {
	return origin_;
}

//! Accessor to the normal of the plane (read-write)
/*!
 @return A reference to the normal of the plane
 */
template <class T>
Vector3d<T>& Plane3d<T>::normal() {
	return normal_;
}

//! Accessor to the normal of the plane (read only)
/*!
 @return A constant reference to the normal of the plane
 */
template <class T>
const Vector3d<T>& Plane3d<T>::normal() const {
	return normal_;
}

//! Test wether a point is inside the plane
/*!
 @param rhs The point to test for coplanarity
 @param tolerance It is unlikely that the point falls exactly on the
                  plane. The tolerance parameter allows for approximate
				  coplanarity
 @return True if the point is inside the plane, false otherwise
 */
template <class T>
bool Plane3d<T>::coplanar(const Point3d<T>& rhs, const T& tolerance) const {
	// Compute the dot product with the given point
	T dot = normal_.dot(Vector3d<T>(origin_, rhs));

	// If the dot product is close enough to 0, point is considered in the plane
	return ( dot <= tolerance );
}

//! Compute the intersection between the line and the plane
/*!
 @param rhs The line to intersect with the plane
 @param line_coordinate The parametric line coordinate of the intersection between
                        the plane and the line, if both do intersect. To retrieve
					    the intersection point, call rhs.point(line_coordinate)
 @param type If not nil, this parameter receives the type of intersection
             between the plane and the line. The intersection type can be:<BR>
			 Parallel if the line is parallel to the plane,<BR>
			 Intersection if the line intersects the plane
 @return True if the line intersects the plane, false otherwise
 @note Type T should have an implicit conversion to type double
 */
template <class T>
bool Plane3d<T>::intersection(const Line3d<T>& rhs, T& line_coordinate, IntersectionType::Type* type) const {
	// Compute denominator
	T den = (
		  rhs.p1().x()*normal_.x() - rhs.p2().x()*normal_.x()
		+ rhs.p1().y()*normal_.y() - rhs.p2().y()*normal_.y()
		+ rhs.p1().z()*normal_.z() - rhs.p2().z()*normal_.z()
	);

	// The line is parallel to the plane
	if ( den == 0 ) {
		if ( type ) *type = IntersectionType::Parallel;
		return false;
	}

	// Compute line coordinate
	line_coordinate = (
		  rhs.p1().x()*normal_.x() - origin_.x()*normal_.x()
		+ rhs.p1().y()*normal_.y() - origin_.y()*normal_.y()
		+ rhs.p1().z()*normal_.z() - origin_.z()*normal_.z()
	) / den;

	// Return intersection result
	if ( type ) *type = IntersectionType::Intersection;
	return true;
}

//! Compute the closest point along the plane
/*!
 @param rhs The point to project on the plane
 @return The closest point along the plane
 */
template <class T>
Point3d<T> Plane3d<T>::closest_point(const Point3d<T>& rhs) const {
	// Get the normal square length
	T den = normal_.square_length();

	// Get coordinate
	T x =   origin_.x()*normal_.x()*normal_.x() + rhs.x()*(normal_.y()*normal_.y() + normal_.z()*normal_.z())
		  + normal_.x()*(origin_.y()*normal_.y() - normal_.y()*rhs.y() + normal_.z()*(origin_.z() - rhs.z()));
	T y =   origin_.x()*normal_.x()*normal_.y() - normal_.x()*rhs.x()*normal_.y()
		  + origin_.y()*normal_.y()*normal_.y() + normal_.x()*normal_.x()*rhs.y()
		  + normal_.z()*(rhs.y()*normal_.z() + normal_.y()*(origin_.z() - rhs.z()));
	T z =   normal_.z()*(origin_.x()*normal_.x() - normal_.x()*rhs.x() + origin_.y()*normal_.y()
				- normal_.y()*rhs.y() + origin_.z()*normal_.z())
		  + (normal_.x()*normal_.x() + normal_.y()*normal_.y())*rhs.z();

	// Return result
	return Point3d<T>(x/den, y/den, z/den);
}

//! Orthogonal projection of the given point along the plane
/*!
 @param rhs The point to project on the plane
 @return The orthogonal projection of the given point along the plane
 @note This is an overloaded function, which is strictly equivalent to
       the closest_point() function
 */
template <class T>
Point3d<T> Plane3d<T>::project(const Point3d<T>& rhs) const {
	return closest_point(rhs);
}

//! Projection of the given point along the plane with given orientation
/*!
 @param rhs The point to project on the plane
 @param direction The orientation of the projection on the plane
 @param impact Receives the projection of the given point along the plane
 @return True if impact could be computed, false otherwise (this function
         returns false only if the projection direction is parallel to the
		 plane)
 */
template <class T>
bool Plane3d<T>::project(const Point3d<T>& rhs, const Vector3d<T>& direction, Point3d<T>& impact) const {
	// Create a line
	Line3d<T> line(rhs, Point3d<T>(rhs.x()+direction.x(), rhs.y()+direction.y(), rhs.z()+direction.z()));

	// Get the intersection between the line and the plane
	T line_coordinate;
	bool ok = intersection(line, line_coordinate);
	if ( ok == false ) return false;

	// Return impact
	impact = line.point(line_coordinate);
	return true;
}

//! Check if the plane is degenerate
/*!
 @param tolerance Allows for approximately degenerate planes
 @return True if the plane is degenerate, false otherwise
 @note The plane is considered degenerate if the square length
       of its normal is equal or below the tolerance threshold
 */
template <class T>
bool Plane3d<T>::degenerate(const T& tolerance) const {
	return ( normal_.get_square_length() <= tolerance );
}

#ifdef GAMEENGINE_NAMESPACES
} // namespace GameEngine
#endif

#endif
